#ifndef MATCH_H
#define MATCH_H

#include <Eigen/Dense>

#include "common/algorithm_type/all_type.h"
// #include "common/log.h"
#include "l_shape_tracker.h"
namespace perception
{
    namespace algorithm
    {
        using namespace Eigen;
        enum track_type
        {
            MATCHED = 1,
            NEW_OBJ = 2,
            MATCHING_FAILED = 3
        };
        class Match
        {
        public:
            Match(track_config &config);
            unsigned long int id;  // identifier for the cluster
            unsigned long int age; // age of the cluster

            void Init(unsigned long int id, LidarObject &new_points, const double &dt, const Localization &ego_pose);
            void update(LidarObject &new_points, const double &dt, const Localization &ego_pose);
            void update2(LidarObject &new_points, const double &dt, const Localization &ego_pose);
            void update(const double &dt, const Localization &ego_pose);
            LshapeTracker Lshape;

            double confidence;
            double max_confidence = 10;
            double dconfidence = 1;

            double old_thetaL1;
            double L1, L2, thetaL1, thetaL2;
            double cx, cy, cvx, cvy, L1_box, L2_box, th, psi, comega, length_box, width_box, height_box; // psi=heading
            double ax, ay;
            double x_ukf, y_ukf, vx_ukf, vy_ukf, omega_ukf;

            double x;
            double y;
            double z;
            // double
            int type;
            LidarObject pre_obj;
            CornerPoint cp;
            // mean value of the cluster

            int track_type_ = track_type::NEW_OBJ;

            std::vector<CornerPoint> all_points;
            float sum_x = 0;
            float sum_y = 0;
            float pre_heading;

        private:
            float now_v_ratio = 0.8;
            float max_a = 10;
            bool CheckStatic();
        };
    }
}
#endif